/*********************************************************************
*                SEGGER MICROCONTROLLER GmbH & Co. KG                *
*        Solutions for real time microcontroller applications        *
**********************************************************************
*                                                                    *
*        (c) 2003-2011     SEGGER Microcontroller GmbH & Co KG       *
*                                                                    *
*        Internet: www.segger.com    Support:  support@segger.com    *
*                                                                    *
**********************************************************************

----------------------------------------------------------------------
File    : RTOSInit.c   (for NXP LPC2478 CPU)
Purpose : Initializes and handles the hardware for the OS as far
          as required by the OS.
          Feel free to modify this file acc. to your
          target system.
Literature:  [1]: NXP ERRATA-SHEET LPC2468 Ver. 1.3
--------  END-OF-HEADER  ---------------------------------------------
*/

#include "stdio.H"
#include "RTOS.H"

/*********************************************************************
*
*       Configuration
*
*********************************************************************/

#define   SDRAM_BASE_ADDR  (0xA0000000)

/*********************************************************************
*
*       Clock frequency settings
*/
#ifndef   OS_USE_INTERNAL_RC
  #define OS_USE_INTERNAL_RC  0
#endif

#ifndef   OS_FSYS                        /* CPU main clock freqeuncy     */
  #define OS_FSYS (72000000uL)           /* may depend on PLL            */
#endif

#ifndef   OS_PCLK_TIMER                  /* Peripheral clock for timer   */
  #define OS_PCLK_TIMER (OS_FSYS/4)      /* defaults to CPU clock / 2    */
#endif

#ifndef   OS_INIT_PLL                    /* PLL may be initialized       */
  #define OS_INIT_PLL         (1)        /* during __low_level_init      */
#endif

#if (OS_USE_INTERNAL_RC)
  #ifndef   OS_PLL_MULTIPLIER            /* PLL multiplier may be        */
    #define OS_PLL_MULTIPLIER  (36)      /* defined by project settings  */
  #endif
  #ifndef   OS_PLL_DIVIDER               /* PLL multiplier may be        */
    #define OS_PLL_DIVIDER      (1)      /* defined by project settings  */
  #endif
#else
  #ifndef   OS_PLL_MULTIPLIER            /* PLL multiplier may be        */
    #define OS_PLL_MULTIPLIER  (24)      /* defined by project settings  */
  #endif
  #ifndef   OS_PLL_DIVIDER               /* PLL multiplier may be        */
    #define OS_PLL_DIVIDER      (2)      /* defined by project settings  */
  #endif
#endif

#ifndef   OS_INIT_MAM                    /* Memory accelerator           */
  #define OS_INIT_MAM              (1)   /* initialized per default      */
#endif

/****** OS internal timing settings ******************************/

#define OS_TIMER_INTERVAL (OS_PCLK_TIMER / 1000)


#if OS_INIT_MAM                     /* Define timer value for MAM   */
  #if   (OS_FSYS < 2000000)
    #define OS_MAMTIM_VALUE    (2)
  #elif (OS_FSYS < 4000000)
    #define OS_MAMTIM_VALUE    (3)
  #else
    #define OS_MAMTIM_VALUE    (5)
  #endif
#endif

/*********************************************************************
*
*       Configuration of communication to OSView
*/
#ifndef   OS_VIEW_ENABLE            // Global enable of communication
  #define OS_VIEW_ENABLE    (1)     // Default: on
#endif

#ifndef   OS_VIEW_USE_UART          // If set, UART will be used
  #define OS_VIEW_USE_UART  (0)     // Default: 0 => DCC is used
#endif                              // if OS_VIEW_ENABLE is on

/*********************************************************************
*
*       UART settings for OSView
*       If you do not want (or can not due to hardware limitations)
*       to dedicate a UART to OSView, please define it to be -1
*       Currently UART0 and UART1 are supported and the standard
*       setup enables UART 0 per default
*       When using DCC for communiction, the UART is not used for embOSView,
*       regardless the OS_UART settings
*/
#ifndef   OS_UART
  #define OS_UART     (-1)
#endif

#ifndef   OS_PCLK_UART                   /* Peripheral clock for UART    */
  #define OS_PCLK_UART (OS_FSYS/4)       /* defaults to CPU clock / 2    */
#endif

#ifndef   OS_BAUDRATE
  #define OS_BAUDRATE (38400uL)
#endif

/****** Define behavior of undefined interrupt handling *************/

#ifndef   OS_IGNORE_UNDEFINED_INTERRUPT
  #define OS_IGNORE_UNDEFINED_INTERRUPT 0
#endif

/****** End of configuration settings *******************************/

/*********************************************************************
*
*       DCC and UART settings for OSView
*
*       Automatically generated from configuration settings
*/
#define OS_USE_DCC     (OS_VIEW_ENABLE && (OS_VIEW_USE_UART == 0))

#define OS_UART_USED   (OS_VIEW_ENABLE && (OS_VIEW_USE_UART != 0)) && (OS_UART == 0)

/********************************************************************/

#if (DEBUG || OS_USE_DCC)
  #include "JLINKDCC.h"
#endif

/****** Configuration check  ****************************************/

#if   (OS_FSYS == OS_PCLK_TIMER)
  #define PCLKSEL_VALUE   (0x55555555)
#elif ((OS_FSYS/OS_PCLK_TIMER) == 2)
  #define PCLKSEL_VALUE   (0xAAAAAAAA)
#elif ((OS_FSYS/OS_PCLK_TIMER) == 4)
  #define PCLKSEL_VALUE   (0x00000000)
#else
  #error "Unsupported peripheral clock divider, code has to be modified"
#endif

#if (OS_PCLK_TIMER != OS_PCLK_UART)
  #error "Different peripheral clocks for UART and timer, code has to be modified"
#endif

/*********************************************************************
*
*       Local defines (sfrs used in RTOSInit.c)
*
**********************************************************************
*/

/****** Port, port mode settings ************************************/

#define PINSEL_BASE_ADDR        (0xE002C000)
#define PINMODE_BASE_ADDR       (0xE002C040)

#define PINSEL0                 *(volatile OS_U32*)(PINSEL_BASE_ADDR + 0x00)
#define PINSEL4                 *(volatile OS_U32*)(PINSEL_BASE_ADDR + 0x10)
#define PINSEL5                 *(volatile OS_U32*)(PINSEL_BASE_ADDR + 0x14)
#define PINSEL6                 *(volatile OS_U32*)(PINSEL_BASE_ADDR + 0x18)
#define PINSEL7                 *(volatile OS_U32*)(PINSEL_BASE_ADDR + 0x1C)
#define PINSEL8                 *(volatile OS_U32*)(PINSEL_BASE_ADDR + 0x20)
#define PINSEL9                 *(volatile OS_U32*)(PINSEL_BASE_ADDR + 0x24)

#define PINMODE5                *(volatile OS_U32*)(PINMODE_BASE_ADDR + 0x14)
#define PINMODE6                *(volatile OS_U32*)(PINMODE_BASE_ADDR + 0x18)
#define PINMODE8                *(volatile OS_U32*)(PINMODE_BASE_ADDR + 0x20)
#define PINMODE9                *(volatile OS_U32*)(PINMODE_BASE_ADDR + 0x24)

#define SCS                     *(volatile OS_U32*)0xE01FC1A0


/****** External memory controller **********************************/

#define EMC_BASE_ADDR           (0xFFE08000)

#define EMCCONTROL              *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0000)
#define EMCDYNAMIC_CONTROL      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0020)
#define EMCDYNAMIC_REFRESH      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0024)
#define EMCDYNAMIC_READCONFIG   *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0028)
#define EMCDYNAMIC_RP           *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0030)
#define EMCDYNAMIC_RAS          *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0034)
#define EMCDYNAMIC_SREX         *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0038)
#define EMCDYNAMIC_APR          *(volatile unsigned long *)(EMC_BASE_ADDR + 0x003C)
#define EMCDYNAMIC_DAL          *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0040)
#define EMCDYNAMIC_WR           *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0044)
#define EMCDYNAMIC_RC           *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0048)
#define EMCDYNAMIC_RFC          *(volatile unsigned long *)(EMC_BASE_ADDR + 0x004C)
#define EMCDYNAMIC_XSR          *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0050)
#define EMCDYNAMIC_RRD          *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0054)
#define EMCDYNAMIC_MRD          *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0058)
#define EMCDYNAMIC_CONFIG0      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0100)
#define EMCDYNAMIC_RASCAS0      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0104)

#define EMCSTATIC_CONFIG0       *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0200)
#define EMCSTATIC_WAITWEN0      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0204)
#define EMCSTATIC_WAITOEN0      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0208)
#define EMCSTATIC_WAITRD0       *(volatile unsigned long *)(EMC_BASE_ADDR + 0x020C)
#define EMCSTATIC_WAITPAGE0     *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0210)
#define EMCSTATIC_WAITWR0       *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0214)
#define EMCSTATIC_WAITTURN0     *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0218)
#define EMCSTATIC_CONFIG1       *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0220)
#define EMCSTATIC_WAITWEN1      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0224)
#define EMCSTATIC_WAITOEN1      *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0228)
#define EMCSTATIC_WAITRD1       *(volatile unsigned long *)(EMC_BASE_ADDR + 0x022C)
#define EMCSTATIC_WAITPAGE1     *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0230)
#define EMCSTATIC_WAITWR1       *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0234)
#define EMCSTATIC_WAITTURN1     *(volatile unsigned long *)(EMC_BASE_ADDR + 0x0238)

/****** Power, reset clock control unit register ********************/

#define PLLCON                  *(volatile OS_U32*)0xE01FC080
#define PLLCFG                  *(volatile OS_U32*)0xE01FC084
#define PLLSTAT                 *(volatile OS_U32*)0xE01FC088
#define PLLFEED                 *(volatile OS_U32*)0xE01FC08C
#define PCON                    *(volatile OS_U32*)0xE01FC0C0
#define PCONP                   *(volatile OS_U32*)0xE01FC0C4
#define CPUCLKCFG               *(volatile OS_U32*)0xE01FC104
#define USBCLKCFG               *(volatile OS_U32*)0xE01FC108
#define CLKSRCSEL               *(volatile OS_U32*)0xE01FC10C
#define PCLKSEL0                *(volatile OS_U32*)0xE01FC1A8
#define PCLKSEL1                *(volatile OS_U32*)0xE01FC1AC

/****** PLL and memeory timing **************************************/

#define MAMCR                   *(volatile OS_U32*)(0xE01FC000)
#define MAMTIM                  *(volatile OS_U32*)(0xE01FC004)

/****** Timer sfr definition ****************************************/

#define TIM0_BASE_ADDR          0xE0004000
#define TIM1_BASE_ADDR          0xE0008000

#define TIM_IR_OFFS             0x00
#define TIM_TCR_OFFS            0x04
#define TIM_TC_OFFS             0x08
#define TIM_PR_OFFS             0x0C
#define TIM_MCR_OFFS            0x14
#define TIM_MR0_OFFS            0x18

/****** Assign timer sfrs used for OS timer **********************/
/****** initially, we use timer 0        ****************************/

#define OS_TIM_BASE_ADDR        TIM0_BASE_ADDR
#define OS_TIM_IR_BIT           0  // Used to reset timer interrupt (Timer IR register), Match channel 0
#define TIMER_PCONP_BIT         1

#define OS_TIMER_INT_INDEX      4 // Timer 0 interrupt

#define OS_TIM_IR               *(volatile OS_U32*)(OS_TIM_BASE_ADDR + TIM_IR_OFFS)
#define OS_TIM_TCR              *(volatile OS_U32*)(OS_TIM_BASE_ADDR + TIM_TCR_OFFS)
#define OS_TIM_TC               *(volatile OS_U32*)(OS_TIM_BASE_ADDR + TIM_TC_OFFS)
#define OS_TIM_PR               *(volatile OS_U32*)(OS_TIM_BASE_ADDR + TIM_PR_OFFS)
#define OS_TIM_MCR              *(volatile OS_U32*)(OS_TIM_BASE_ADDR + TIM_MCR_OFFS)
#define OS_TIM_MR0              *(volatile OS_U32*)(OS_TIM_BASE_ADDR + TIM_MR0_OFFS)

/****** Vectored interrupt controller (VIC) *************************/

#define VIC_BASE_ADDR           0xFFFFF000

#define VIC_IRQSTATUS           *(volatile OS_U32*)(VIC_BASE_ADDR + 0x000)
#define VIC_FIQSTATUS           *(volatile OS_U32*)(VIC_BASE_ADDR + 0x004)
#define VIC_RAWINTERRUPT        *(volatile OS_U32*)(VIC_BASE_ADDR + 0x008)
#define VIC_INTSELECT           *(volatile OS_U32*)(VIC_BASE_ADDR + 0x00C)
#define VIC_INTENABLE           *(volatile OS_U32*)(VIC_BASE_ADDR + 0x010)
#define VIC_INTENABLECLEAR      *(volatile OS_U32*)(VIC_BASE_ADDR + 0x014)
#define VIC_SOFTINT             *(volatile OS_U32*)(VIC_BASE_ADDR + 0x018)
#define VIC_SOFTINTCLEAR        *(volatile OS_U32*)(VIC_BASE_ADDR + 0x01C)

#define VIC_PRIO_0              *(volatile OS_U32*)(VIC_BASE_ADDR + 0x200)
#define VIC_VECTORADDR          *(volatile OS_U32*)(VIC_BASE_ADDR + 0xF00)

#define VIC_VECT_0              *(volatile OS_U32*)(VIC_BASE_ADDR + 0x100)
#define VIC_VECT_BASE_ADDR      (VIC_BASE_ADDR + 0x100)
#define VIC_PRIO_BASE_ADDR      (VIC_BASE_ADDR + 0x200)

#ifndef   NUM_INT_VECTORS
  #define NUM_INT_VECTORS       32
#endif

#define NUM_INT_PRIORITIES      (16)
#define INT_PRIORITY_MASK       (NUM_INT_PRIORITIES - 1)

/*********************************************************************
*
*       Static data
*
**********************************************************************
*/

/*********************************************************************
*
*       Local functions
*
**********************************************************************
*/

/*********************************************************************
*
*       _InitPLL
*
* Function description
*   Initialize PLL
*   WARNING: Improper settings may lock CPU
*   Please examine whether init sequence fits your harware configuration
*   We assume a CPU running with internal oscillator.
*/
static void _InitPLL(void) {
  if (PLLSTAT & (1 << 25)) {
    PLLCON  |= (1 << 1);
    PLLFEED  = 0xAA;
    PLLFEED  = 0x55;
  }
  PLLCON     = 0;                         // Set PLL disabled and disconnected
  PLLFEED    = 0xAA;
  PLLFEED    = 0x55;
#if (OS_USE_INTERNAL_RC)
  CLKSRCSEL  = 0;                         // Select internal RC oscillator 4 MHz as clock source
#else
  SCS        &= ~(1UL << 4);                // The frequency range of the main oscillator is 1 MHz to 20 MHz.
  SCS        |=  (1 << 5);                // Main oscillator enable.
  CLKSRCSEL  = 1;                         // Select main oscillator 12 MHz as clock source
#endif
  PLLCFG     =  (OS_PLL_MULTIPLIER - 1)    |  // MSEL: Multiplier - 1
               ((OS_PLL_DIVIDER - 1) << 16);  // NSEL: Divider - 1
  PLLFEED    = 0xAA;
  PLLFEED    = 0x55;
  PLLCON     = 1;                         // Enable PLL, Disconnect
  PLLFEED    = 0xAA;
  PLLFEED    = 0x55;
  do {
  } while ((PLLSTAT & (1 << 26)) == 0);   // Check PLOCK bit	
  CPUCLKCFG  = 3;                         // Set CPU clock divider, value = Division ratio - 1)
  USBCLKCFG  = 5;                         // Set USB clock divider, value = Division ratio - 1)

  PCLKSEL0   = PCLKSEL_VALUE;
  PCLKSEL1   = PCLKSEL_VALUE;

  PLLCON     = (1 << 0) | (1 << 1);       // Enable PLL and select as clock source
  PLLFEED    =  0xAA;
  PLLFEED    =  0x55;
}

/*********************************************************************
*
*       _Init_VIC()
*
* Function description
*   Clears and disables all interrut vectors in VIC.
*/
static void _Init_VIC(void) {
  int Index;

  VIC_INTENABLECLEAR = 0xFFFFFFFF;            // Disable all source interrupts
  VIC_VECTORADDR     = 0;                     // Write to vector, reset VIC
  for (Index = 0; Index < NUM_INT_VECTORS; Index++) {
    OS_ARM_InstallISRHandler(Index, NULL);    // Set vector to 0;
  }
  VIC_SOFTINTCLEAR   = 0xFFFFFFFF;            // Disable all soft interrupts
  VIC_INTSELECT      = 0;                     // Set all channels to generate IRQ
}

/*********************************************************************
*
*       OS_ISR_Tick() interrupt Handler
*/
static void _OS_ISR_Tick(void) {
  OS_TIM_IR = (1 << OS_TIM_IR_BIT);  // Clear OS timer interrupt flag
  OS_HandleTick();                     // Call OS tick handler
#if (DEBUG || OS_USE_DCC)
  DCC_Process();
#endif
}

/*********************************************************************
*
*       _EMC_Init()
*
*  Purpose:
*    Initializes external memory controller for SDRAM, NAND- and NOR-flash
*/

typedef float                 Flo32;    // Single precision floating point
#define SDRAM_PERIOD          17.4  // 57.6MHz
#define P2C(Period)           (((Period<SDRAM_PERIOD)?0:(OS_U32)((Flo32)Period/SDRAM_PERIOD))+1)

static void _SDRAM_Init(void) {
  volatile OS_U32 i;
  volatile OS_U32 Dummy;

  PINSEL5  = (PINSEL5 & 0xF0FFCFCC0)
             |   (1 <<  0)  // CAS
             |   (1 <<  2)  // RAS
             |   (1 <<  4)  // CLKOUT0
             |   (1 <<  8)  // DYCS0
             |   (1 << 16)  // CKEOUT0
             |   (1 << 24)  // DQMOUT0
             |   (1 << 26)  // DQMOUT1
             ;
  PINMODE5 = (PINMODE5 & 0xF0FFCFCC0)
             | (0x2 <<  0)  // Pin floating
             | (0x2 <<  2)  // Pin floating
             | (0x2 <<  4)  // Pin floating
             | (0x2 <<  8)  // Pin floating
             | (0x2 << 16)  // Pin floating
             | (0x2 << 24)  // Pin floating
             | (0x2 << 26)  // Pin floating
             ;
  PINSEL6  = 0
             |   (1 <<  0)  // D0
             |   (1 <<  2)  // D1
             |   (1 <<  4)  // D2
             |   (1 <<  6)  // D3
             |   (1 <<  8)  // D4
             |   (1 << 10)  // D5
             |   (1 << 12)  // D6
             |   (1 << 14)  // D7
             |   (1 << 16)  // D8
             |   (1 << 18)  // D9
             |   (1 << 20)  // D10
             |   (1 << 22)  // D11
             |   (1 << 24)  // D12
             |   (1 << 26)  // D13
             |   (1 << 28)  // D14
             |   (1 << 30)  // D15
             ;
  PINMODE6 = 0
             | (0x2   <<  0)  // Pin floating
             | (0x2   <<  2)  // Pin floating
             | (0x2   <<  4)  // Pin floating
             | (0x2   <<  6)  // Pin floating
             | (0x2   <<  8)  // Pin floating
             | (0x2   << 10)  // Pin floating
             | (0x2   << 12)  // Pin floating
             | (0x2   << 14)  // Pin floating
             | (0x2   << 16)  // Pin floating
             | (0x2   << 18)  // Pin floating
             | (0x2   << 20)  // Pin floating
             | (0x2   << 22)  // Pin floating
             | (0x2   << 24)  // Pin floating
             | (0x2   << 26)  // Pin floating
             | (0x2   << 28)  // Pin floating
             | (0x2uL << 30)  // Pin floating
             ;
  PINSEL8  = (PINSEL8 & 0xC0000000)
             |   (1 <<  0)  // A0
             |   (1 <<  2)  // A1
             |   (1 <<  4)  // A2
             |   (1 <<  6)  // A3
             |   (1 <<  8)  // A4
             |   (1 << 10)  // A5
             |   (1 << 12)  // A6
             |   (1 << 14)  // A7
             |   (1 << 16)  // A8
             |   (1 << 18)  // A9
             |   (1 << 20)  // A10
             |   (1 << 22)  // A11
             |   (1 << 24)  // A12
             |   (1 << 26)  // A13
             |   (1 << 28)  // A14
             ;
  PINMODE8 = (PINMODE8 & 0xC0000000)
             | (0x2 <<  0)  // A0
             | (0x2 <<  2)  // A1
             | (0x2 <<  4)  // A2
             | (0x2 <<  6)  // A3
             | (0x2 <<  8)  // A4
             | (0x2 << 10)  // A5
             | (0x2 << 12)  // A6
             | (0x2 << 14)  // A7
             | (0x2 << 16)  // A8
             | (0x2 << 18)  // A9
             | (0x2 << 20)  // A10
             | (0x2 << 22)  // A11
             | (0x2 << 24)  // A12
             | (0x2 << 26)  // A13
             | (0x2 << 28)  // A14
             ;
  PINSEL9  = (PINSEL9 & 0xFFF3FFFF)
             |   (1 << 18)  // WE
             ;
  PINMODE9 = (PINMODE9 & 0xFFF3FFFF)
             | (0x2 << 18)  // Pin floating
             ;

  EMCCONTROL  = (1 << 0);    // EMC enable
  PCONP      |= (1 << 11);   // Turn on EMC peripheral clock
  EMCDYNAMIC_READCONFIG = 1;
  EMCDYNAMIC_RASCAS0 = 0
                       | (0x3 << 0)  // RAS latency, two CCLK cycles
                       | (0x3 << 8)  // CAS latency, two CCLK cycles
                       ;
  EMCDYNAMIC_RP      = P2C(20);
  EMCDYNAMIC_RAS     = P2C(45);
  EMCDYNAMIC_SREX    = P2C(67);
  EMCDYNAMIC_APR     = 1;
  EMCDYNAMIC_DAL     = 3 + P2C(20);
  EMCDYNAMIC_WR      = 3;
  EMCDYNAMIC_RC      = P2C(65);
  EMCDYNAMIC_RFC     = P2C(66);
  EMCDYNAMIC_XSR     = P2C(67);
  EMCDYNAMIC_RRD     = P2C(15);
  EMCDYNAMIC_MRD     = 3;
  EMCDYNAMIC_CONFIG0 = 0x0680;       // Row length = 13, column length = 9
  EMCDYNAMIC_CONTROL = 0
                       | (1   << 0)  // Dynamic memory clock enable (CE), all clock enables are driven high continuously
                       | (1   << 1)  // Dynamic memory clock control (CS), CLKOUT runs continuously
                       | (0x3 << 7)  // Issue SDRAM NOP command
                       ;
  for (i = 200*30; i; i--);  // Delay
  EMCDYNAMIC_CONTROL = (EMCDYNAMIC_CONTROL & ~(0x3UL << 7))
                       | (0x2 << 7)  // Issue SDRAM PALL (precharge all) command
                       ;
  EMCDYNAMIC_REFRESH = 0x01;         // n x 16 CCLKs between SDRAM refresh cycles
  for (i = 128; i; --i);             // > 128 clk
  EMCDYNAMIC_REFRESH = P2C(7813) >> 4;
  EMCDYNAMIC_CONTROL = (EMCDYNAMIC_CONTROL & ~(0x3UL << 7))
                       | (0x1 << 7)  // Issue SDRAM MODE command
                       ;
  Dummy = *((volatile OS_U16*)((OS_U32)SDRAM_BASE_ADDR + (0x33 << 12)));  //Set mode register in SDRAM
  EMCDYNAMIC_CONTROL  = 0x0;
  EMCDYNAMIC_CONFIG0 |= (1 << 19);   // Enable buffer
  for (i = 10000; i; i--);           //Delay
}

/*********************************************************************
*
*       Global functions
*
**********************************************************************
*/

/*********************************************************************
*
*       OS_InitHW()
*
*       Initialize the hardware (timer) required for the OS to run.
*       May be modified, if an other timer should be used
*       Also enables memory accelerator module
*/
void OS_InitHW(void) {
  OS_DI();
  _Init_VIC();                   // Initialize VIC, clear and disable all interrupts
  // Initialize timer for OS
  PCONP      |=  (1 << TIMER_PCONP_BIT);   // Enable Clock for OS timer
  OS_TIM_TCR &= ~(1UL << 0);                  // Disable timer
  OS_TIM_IR   =  (1 << OS_TIM_IR_BIT);     // Clear OS timer interrupt flag
  OS_TIM_PR   = 0;                          // Count on every pclk
  OS_TIM_MR0  = OS_TIMER_INTERVAL - 1;     // Initialize match register 1
  OS_TIM_MCR  =  (1 << 0)                   // Interrupt on match channel 0
               | (1 << 1);                  // Counter reset on match channel 0
  OS_TIM_TC   =  0;                         // Clear counter
  OS_TIM_TCR |=  (1 << 0);                  // Enable timer
  // Setup interrupt controller
  OS_ARM_InstallISRHandler(OS_TIMER_INT_INDEX, &_OS_ISR_Tick); // Timer/counter interrupt vector.
  OS_ARM_EnableISR(OS_TIMER_INT_INDEX);                        // Enable timer/counter 0 interrupt.
  OS_ARM_ISRSetPrio(OS_TIMER_INT_INDEX, 15);                   // lowest priority
  // optionally initialize UART for OSView
  OS_COM_Init();
  OS_RestoreI();
}

/*********************************************************************
*
*       Idle loop  (OS_Idle)
*
*       Please note:
*       This is basically the "core" of the idle loop.
*       This core loop can be changed, but:
*       The idle loop does not have a stack of its own, therefore no
*       functionality should be implemented that relies on the stack
*       to be preserved. However, a simple program loop can be programmed
*       (like toggeling an output or incrementing a counter)
*/
void OS_Idle(void) {     // Idle loop: No task is ready to exec
  while (1) {
  }
}

/*********************************************************************
*
*       Get time [cycles]
*
*       This routine is required for task-info via OSView or high
*       resolution time measurement functions.
*       It returns the system time in timer clock cycles.
*/
OS_U32 OS_GetTime_Cycles(void) {
  OS_U32 time;
  OS_U32 count;

  count = OS_TIM_TC;                               // Read current timer value
  time  = OS_GetTime32();                           // Read current OS time
  if ((OS_TIM_IR & (1 << OS_TIM_IR_BIT)) != 0) {  // Timer Interrupt pending ?
    time++;                                         // Adjust result, read timer again
    count = OS_TIM_TC;                             // Read again
  }
  return (OS_TIMER_INTERVAL) * time + count;
}

/*********************************************************************
*
*       OS_ConvertCycles2us
*
*       Convert Cycles into micro seconds.
*
*       If your clock frequency is not a multiple of 1 MHz,
*       you may have to modify this routine in order to get proper
*       diagonstics.
*
*       This routine is required for profiling or high resolution time
*       measurement only. It does not affect operation of the OS.
*/
OS_U32 OS_ConvertCycles2us(OS_U32 Cycles) {
  return Cycles/(OS_PCLK_TIMER/1000000);
}

/*********************************************************************
*
*       Communication for OSView via UART (optional)
*
**********************************************************************
*/

#if OS_UART_USED
  #error "UART for embOSView not supported in this version!!
#elif OS_USE_DCC

/*********************************************************************
*
*       _DCC_OnRx()
*/
static void _DCC_OnRx(unsigned Channel, OS_U8 Data) {
  (void)Channel;
  OS_OnRx(Data);
}

/*********************************************************************
*
*       _DCC_OnTx()
*/
static void _DCC_OnTx(unsigned Channel) {
  (void)Channel;
  OS_OnTx();
}

/*********************************************************************
*
*       OS_COM_Init()
*       Initialize DCC for OSView
*/
void OS_COM_Init(void) {
  JLINKDCC_SetpfOnRx(1, _DCC_OnRx);
  JLINKDCC_SetpfOnTx(1, _DCC_OnTx);
}

/*********************************************************************
*
*       OS_COM_Send1()
*       Send 1 character via DCC
*/
void OS_COM_Send1(OS_U8 c) {
  JLINKDCC_SendCharOnChannelNB(1, c);
}

#else  /* DCC not selected, selected UART not supported, using dummies */

void OS_COM_Init(void) {}
void OS_COM_Send1(OS_U8 c) {
  OS_USEPARA(c);           /* avoid compiler warning */
  OS_COM_ClearTxActive();  /* let the OS know that Tx is not busy */
}

#endif /*  OS_UART_USED  */

/****** Final check of configuration ********************************/

#ifndef OS_UART_USED
  #error "OS_UART_USED has to be defined"
#endif

/*********************************************************************
*
*       OS interrupt handler and ISR specific functions
*
**********************************************************************
*/

/*********************************************************************
*
*       _OS_ISR_Undefined
*
*       Is called when an uninstalled interrupt was detected
*       As interrupt pending condition of peripherals has to be reset,
*       program will not continue when interrupt is ignored.
*/
static void _OS_ISR_Undefined(void) {
  #if (OS_IGNORE_UNDEFINED_INTERRUPT == 0)
    volatile OS_U32 Dummy;

    Dummy = 1;
    /* You may set a breakpoint here to detect undefined interrupts */
    while (Dummy > 0) {
    }
  #endif
}

/*********************************************************************
*
*       OS_irq_handler
*
*       Detect reason for IRQ and call correspondig service routine.
*       OS_irq_handler is called from OS_IRQ_SERVICE function
*       found in RTOSVect.asm
*/
OS_INTERWORK void OS_irq_handler(void) {
  OS_ISR_HANDLER* pISR;
  pISR = (OS_ISR_HANDLER*) VIC_VECTORADDR;   // Get current interrupt handler
  OS_EnterNestableInterrupt();                // Now interrupts may be reenabled. If nesting should be allowed
  if (pISR != NULL) {
    pISR();                                   // Call installed interrupt service routine
  } else {
    _OS_ISR_Undefined();
  }
  OS_DI();                                    // Disable interrupts and unlock
  VIC_VECTORADDR = 0;                        // Clear current interrupt pending condition, reset VIC
  OS_LeaveNestableInterrupt();                // Replace by OS_LeaveInterrupt(), when nesting was disabled
}

/*********************************************************************
*
*       OS_ARM_InstallISRHandler
*/
OS_ISR_HANDLER* OS_ARM_InstallISRHandler (int ISRIndex, OS_ISR_HANDLER* pISRHandler) {
  OS_ISR_HANDLER*  pOldHandler;
  OS_ISR_HANDLER** papISR;

#if DEBUG
  if ((unsigned)ISRIndex >= NUM_INT_VECTORS) {
    OS_Error(OS_ERR_ISR_INDEX);
    return NULL;
  }
#endif
  OS_DI();
  papISR = (OS_ISR_HANDLER**) &VIC_VECT_0;
  pOldHandler          = *(papISR + ISRIndex);
  *(papISR + ISRIndex) = pISRHandler;
  OS_RestoreI();
  return pOldHandler;
}

/*********************************************************************
*
*       OS_ARM_EnableISR
*/
void OS_ARM_EnableISR(int ISRIndex) {
#if DEBUG
  if ((unsigned)ISRIndex >= NUM_INT_VECTORS) {
    OS_Error(OS_ERR_ISR_INDEX);
    return;
  }
#endif
  OS_DI();
  VIC_INTENABLE = (1UL << ISRIndex);
  OS_RestoreI();
}

/*********************************************************************
*
*       OS_ARM_DisableISR
*/
void OS_ARM_DisableISR(int ISRIndex) {
#if DEBUG
  if ((unsigned)ISRIndex >= NUM_INT_VECTORS) {
    OS_Error(OS_ERR_ISR_INDEX);
    return;
  }
#endif
  OS_DI();
  VIC_INTENABLECLEAR = (1UL << ISRIndex);
  OS_RestoreI();
}

/*********************************************************************
*
*       OS_ARM_ISRSetPrio
*/
int OS_ARM_ISRSetPrio(int ISRIndex, int Prio) {
  OS_U32* pPrio;
  int     OldPrio;
#if DEBUG
  if ((unsigned)ISRIndex >= NUM_INT_VECTORS) {
    OS_Error(OS_ERR_ISR_INDEX);
    return 0;
  }
#endif
  OS_DI();
  pPrio = (OS_U32*) VIC_PRIO_BASE_ADDR;
  OldPrio = pPrio[ISRIndex];
  pPrio[ISRIndex] = (OldPrio & ~INT_PRIORITY_MASK) | (Prio & INT_PRIORITY_MASK);
  OS_RestoreI();
  return OldPrio & INT_PRIORITY_MASK;
}

#ifdef __cplusplus
extern "C" {
#endif

/*********************************************************************
*
*       __low_level_init()
*
* Function description
*   Called from startup code.
*   Used to initialize PLL and memory accelerator as early as possible
*/
OS_INTERWORK int __low_level_init(void);       /* Avoid "No prototype" warning */
OS_INTERWORK int __low_level_init(void) {
  #if OS_INIT_PLL
    _InitPLL();
  #endif
  #if OS_INIT_MAM                // Speedup CPU
    MAMCR  = 0x00;              // Disable Memory accelerator module
    MAMTIM = OS_MAMTIM_VALUE;  // Depends on OS_FSYS
    MAMCR  = 0x01;              // Enable Memory accelerator module partially , refer to [1], MAM.1 operation
  #endif
  //
  // Init SDRAM, NAND- and NOR-flash
  //
  _SDRAM_Init();

  return 1;                      // Always initialize segments !
}

#ifdef __cplusplus
  }
#endif

/*****  EOF  ********************************************************/

